/**************************************************************************************************
*                                                                                                 *
* This file is part of HPIPM.                                                                     *
*                                                                                                 *
* HPIPM -- High-Performance Interior Point Method.                                                *
* Copyright (C) 2019 by Gianluca Frison.                                                          *
* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
* All rights reserved.                                                                            *
*                                                                                                 *
* The 2-Clause BSD License                                                                        *
*                                                                                                 *
* Redistribution and use in source and binary forms, with or without                              *
* modification, are permitted provided that the following conditions are met:                     *
*                                                                                                 *
* 1. Redistributions of source code must retain the above copyright notice, this                  *
*    list of conditions and the following disclaimer.                                             *
* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
*    this list of conditions and the following disclaimer in the documentation                    *
*    and/or other materials provided with the distribution.                                       *
*                                                                                                 *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
*                                                                                                 *
* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
*                                                                                                 *
**************************************************************************************************/



void OCP_QP_DIM_REDUCE_EQ_DOF(struct OCP_QP_DIM *dim, struct OCP_QP_DIM *dim_red)
	{
	int ii;

	// TODO remove slack variables of soft constraints (also DOF)

	// first stage: DOF: inputs and states
	dim_red->nu[0] = dim->nu[0] - dim->nbue[0];
	dim_red->nx[0] = dim->nx[0] - dim->nbxe[0];
	dim_red->nbu[0] = dim->nbu[0] - dim->nbue[0];
	dim_red->nbx[0] = dim->nbx[0] - dim->nbxe[0];
	dim_red->nb[0] = dim_red->nbu[0] + dim_red->nbx[0];
	dim_red->ng[0] = dim->ng[0];
	dim_red->ns[0] = dim->ns[0];
	dim_red->nsbu[0] = dim->nsbu[0];
	dim_red->nsbx[0] = dim->nsbx[0];
	dim_red->nsg[0] = dim->nsg[0];
	dim_red->nbue[0] = 0;
	dim_red->nbxe[0] = 0;
	dim_red->nge[0] = dim->nge[0];
	// other stages: DOF: inputs
	for(ii=1; ii<=dim->N; ii++)
		{
		dim_red->nu[ii] = dim->nu[ii] - dim->nbue[ii];
		dim_red->nx[ii] = dim->nx[ii];
		dim_red->nbu[ii] = dim->nbu[ii] - dim->nbue[ii];
		dim_red->nbx[ii] = dim->nbx[ii];
		dim_red->nb[ii] = dim_red->nbu[ii] + dim_red->nbx[ii];
		dim_red->ng[ii] = dim->ng[ii];
		dim_red->ns[ii] = dim->ns[ii];
		dim_red->nsbu[ii] = dim->nsbu[ii];
		dim_red->nsbx[ii] = dim->nsbx[ii];
		dim_red->nsg[ii] = dim->nsg[ii];
		dim_red->nbue[ii] = 0;
		dim_red->nbxe[ii] = dim->nbxe[ii];
		dim_red->nge[ii] = dim->nge[ii];
		}
	
	return;
	}



hpipm_size_t OCP_QP_REDUCE_EQ_DOF_ARG_MEMSIZE()
	{

	return 0;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_CREATE(struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, void *mem)
	{

	arg->memsize = OCP_QP_REDUCE_EQ_DOF_ARG_MEMSIZE();

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_SET_DEFAULT(struct OCP_QP_REDUCE_EQ_DOF_ARG *arg)
	{

	arg->lam_min = 1e-16;
	arg->t_min = 1e-16;
	arg->alias_unchanged = 0;
	arg->comp_prim_sol = 1;
	arg->comp_dual_sol_eq = 1;
	arg->comp_dual_sol_ineq = 1;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_SET_ALIAS_UNCHANGED(int *value, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg)
	{

	arg->alias_unchanged = *value;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_SET_COMP_PRIM_SOL(int *value, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg)
	{

	arg->comp_prim_sol = *value;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_SET_COMP_DUAL_SOL_EQ(int *value, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg)
	{

	arg->comp_dual_sol_eq = *value;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_SET_COMP_DUAL_SOL_INEQ(int *value, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg)
	{

	arg->comp_dual_sol_ineq = *value;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_ARG_DEEPCOPY(struct OCP_QP_REDUCE_EQ_DOF_ARG *arg_s, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg_d)
	{
	arg_d->lam_min = arg_s->lam_min;
	arg_d->t_min = arg_s->t_min;
	arg_d->alias_unchanged = arg_s->alias_unchanged;
	arg_d->comp_prim_sol = arg_s->comp_prim_sol;
	arg_d->comp_dual_sol_eq = arg_s->comp_dual_sol_eq;
	arg_d->comp_dual_sol_ineq = arg_s->comp_dual_sol_ineq;
	// TODO keep updated !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	return;
	}



hpipm_size_t OCP_QP_REDUCE_EQ_DOF_WS_MEMSIZE(struct OCP_QP_DIM *dim)
	{

	int ii;

	// extract dim
	int N = dim->N;
	int *nu = dim->nu;
	int *nx = dim->nx;
	int *nb = dim->nb;
	int *ng = dim->ng;

	int nuxM = nu[0]+nx[0];
	int nbgM = nb[0]+ng[0];
	for(ii=1; ii<=N; ii++)
		{
		nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
		nbgM = nb[ii]+ng[ii]>nbgM ? nb[ii]+ng[ii] : nbgM;
		}

	hpipm_size_t size = 0;

	size += nuxM*sizeof(int); // e_imask_ux
	size += nbgM*sizeof(int); // e_imask_d

	size += 3*sizeof(struct STRVEC); // tmp_nuxM(0,1) tmp_nbgM

	size += 2*SIZE_STRVEC(nuxM); // tmp_nuxM(0,1)
	size += 1*SIZE_STRVEC(nbgM); // tmp_nbgM

	size = (size+63)/64*64; // make multiple of typical cache line size
	size += 64; // align to typical cache line size

	return size;

	}



void OCP_QP_REDUCE_EQ_DOF_WS_CREATE(struct OCP_QP_DIM *dim, struct OCP_QP_REDUCE_EQ_DOF_WS *work, void *mem)
	{

	int ii;

	// zero memory (to avoid corrupted memory like e.g. NaN)
	hpipm_size_t memsize = OCP_QP_REDUCE_EQ_DOF_WS_MEMSIZE(dim);
	hpipm_zero_memset(memsize, mem);

	// extract dim
	int N = dim->N;
	int *nu = dim->nu;
	int *nx = dim->nx;
	int *nb = dim->nb;
	int *ng = dim->ng;

	int nuxM = nu[0]+nx[0];
	int nbgM = nb[0]+ng[0];
	for(ii=1; ii<=N; ii++)
		{
		nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
		nbgM = nb[ii]+ng[ii]>nbgM ? nb[ii]+ng[ii] : nbgM;
		}

	// vector struct stuff
	struct STRVEC *sv_ptr = (struct STRVEC *) mem;

	// tmp_nuxM
	work->tmp_nuxM = sv_ptr;
	sv_ptr += 2;
	// tmp_nbgM
	work->tmp_nbgM = sv_ptr;
	sv_ptr += 1;

	// integer stuff
	int *i_ptr;
	i_ptr = (int *) sv_ptr;

	// e_imask_ux
	work->e_imask_ux = i_ptr;
	i_ptr += nuxM;
	// e_imask_d
	work->e_imask_d = i_ptr;
	i_ptr += nbgM;

	// align to typical cache line size
	hpipm_size_t l_ptr = (hpipm_size_t) i_ptr;
	l_ptr = (l_ptr+63)/64*64;

	// floating point stuff
	char *c_ptr;
	c_ptr = (char *) l_ptr;

	// tmp_nuxM
	CREATE_STRVEC(nuxM, work->tmp_nuxM+0, c_ptr);
	c_ptr += (work->tmp_nuxM+0)->memsize;
	CREATE_STRVEC(nuxM, work->tmp_nuxM+1, c_ptr);
	c_ptr += (work->tmp_nuxM+1)->memsize;
	// tmp_nbgM
	CREATE_STRVEC(nbgM, work->tmp_nbgM, c_ptr);
	c_ptr += (work->tmp_nbgM)->memsize;

	work->memsize = memsize;

	return;

	}



void OCP_QP_REDUCE_EQ_DOF(struct OCP_QP *qp, struct OCP_QP *qp_red, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, struct OCP_QP_REDUCE_EQ_DOF_WS *work)
	{

	int ii, jj, kk, idx0, idx1;

	struct OCP_QP_DIM *dim = qp->dim;
	int N = dim->N;
	int *nx = dim->nx;
	int *nu = dim->nu;
	int *nb = dim->nb;
	int *nbx = dim->nbx;
	int *nbu = dim->nbu;
	int *ng = dim->ng;
	int *ns = dim->ns;
	int *nbue = dim->nbue;
	int *nbxe = dim->nbxe;
	int *nge = dim->nge;

	struct OCP_QP_DIM *dim_red = qp_red->dim;
	int *nx_red = dim_red->nx;
	int *nu_red = dim_red->nu;
	int *nb_red = dim_red->nb;
	int *ng_red = dim_red->ng;
	int *ns_red = dim_red->ns;

	// TODO handle case of softed equalities !!!!!!!!!!!!!!!!

	// TODO for the non-reduced constraints, copy the lower inequality to both upper and lower inequalities? In this way only the lower one is ever considered also in case the constraint is not reduced.

	int ne_thr;

	for(ii=0; ii<=N; ii++)
		{
		if(ii==0)
			ne_thr = nbue[ii]+nbxe[ii];
		else
			ne_thr = nbue[ii];
		if(ne_thr>0) // reduce inputs and/or states
			{
			VECSE(nu[ii]+nx[ii], 0.0, work->tmp_nuxM+0, 0);
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				work->e_imask_ux[jj] = 0;
			for(jj=0; jj<nbu[ii]+nbx[ii]; jj++)
				work->e_imask_d[jj] = 0;
			for(jj=0; jj<ne_thr; jj++) // set 1s for both inputs and states
				{
				VECEL(work->tmp_nuxM+0, qp->idxb[ii][qp->idxe[ii][jj]]) = VECEL(qp->d+ii, qp->idxe[ii][jj]);
				work->e_imask_ux[qp->idxb[ii][qp->idxe[ii][jj]]] = 1;
				work->e_imask_d[qp->idxe[ii][jj]] = 1;
				}
			// TODO check first and last non-zero in e_mask and only multiply between them
			if(ii<N)
				{
				// BAt
				idx0 = 0;
				for(jj=0; jj<nu[ii]+nx[ii]; jj++)
					{
					if(work->e_imask_ux[jj]==0)
						{
						GECP(1, nx[ii+1], qp->BAbt+ii, jj, 0, qp_red->BAbt+ii, idx0, 0);
						idx0++;
						}
					}
				// b
				GEMV_T(nu[ii]+nx[ii], nx[ii+1], 1.0, qp->BAbt+ii, 0, 0, work->tmp_nuxM+0, 0, 1.0, qp->b+ii, 0, qp_red->b+ii, 0);
				}
			// RSQ
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
//					idx1 = 0;
					idx1 = idx0;
//					for(kk=0; kk<nu[ii]+nx[ii]; kk++)
					for(kk=jj; kk<nu[ii]+nx[ii]; kk++)
						{
						if(work->e_imask_ux[kk]==0)
							{
							MATEL(qp_red->RSQrq+ii, idx1, idx0) = MATEL(qp->RSQrq+ii, kk, jj);
//#endif
							idx1++;
							}
						}
					idx0++;
					}
				}
			// rq
			SYMV_L(nu[ii]+nx[ii], 1.0, qp->RSQrq+ii, 0, 0, work->tmp_nuxM+0, 0, 1.0, qp->rqz+ii, 0, work->tmp_nuxM+1, 0);
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
					VECEL(qp_red->rqz+ii, idx0) = VECEL(work->tmp_nuxM+1, jj);
					idx0++;
					}
				}
			// d d_mask m idxb idxs_rev
			idx0 = 0;
			for(jj=0; jj<nb[ii]; jj++)
				{
				if(work->e_imask_d[jj]==0)
					{
					VECEL(qp_red->d+ii, idx0) = VECEL(qp->d+ii, jj);
					VECEL(qp_red->d+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->d+ii, nb[ii]+ng[ii]+jj);
					VECEL(qp_red->d_mask+ii, idx0) = VECEL(qp->d_mask+ii, jj);
					VECEL(qp_red->d_mask+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->d_mask+ii, nb[ii]+ng[ii]+jj);
					VECEL(qp_red->m+ii, idx0) = VECEL(qp->m+ii, jj);
					VECEL(qp_red->m+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->m+ii, nb[ii]+ng[ii]+jj);
					qp_red->idxb[ii][idx0] = qp->idxb[ii][jj];
					qp_red->idxs_rev[ii][idx0] = qp->idxs_rev[ii][jj]; // keep softed inequality constr with same slack
					idx0++;
					}
				}
			VECCP(ng[ii], qp->d+ii, nb[ii], qp_red->d+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->d+ii, 2*nb[ii]+ng[ii], qp_red->d+ii, 2*nb_red[ii]+ng_red[ii]);
			VECCP(ng[ii], qp->d_mask+ii, nb[ii], qp_red->d_mask+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->d_mask+ii, 2*nb[ii]+ng[ii], qp_red->d_mask+ii, 2*nb_red[ii]+ng_red[ii]);
			VECCP(ng[ii], qp->m+ii, nb[ii], qp_red->m+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->m+ii, 2*nb[ii]+ng[ii], qp_red->m+ii, 2*nb_red[ii]+ng_red[ii]);
			GEMV_T(nu[ii]+nx[ii], ng[ii], 1.0, qp->DCt+ii, 0, 0, work->tmp_nuxM+0, 0, 0.0, work->tmp_nbgM, 0, work->tmp_nbgM, 0);
			AXPY(ng[ii], -1.0, work->tmp_nbgM, 0, qp->d+ii, nb[ii], qp_red->d+ii, nb_red[ii]);
			AXPY(ng[ii], 1.0, work->tmp_nbgM, 0, qp->d+ii, 2*nb[ii]+ng[ii], qp_red->d+ii, 2*nb_red[ii]+ng_red[ii]);
			// DCt
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
					GECP(1, ng[ii], qp->DCt+ii, jj, 0, qp_red->DCt+ii, idx0, 0);
					idx0++;
					}
				}
			// soft constraints
			for(jj=0; jj<ng[ii]; jj++)
				qp_red->idxs_rev[ii][nb_red[ii]+jj] = qp->idxs_rev[ii][nb[ii]+jj]; // keep softed inequality constr with same slack
			VECCP(2*ns[ii], qp->Z+ii, 0, qp_red->Z+ii, 0);
			VECCP(2*ns[ii], qp->rqz+ii, nu[ii]+nx[ii], qp_red->rqz+ii, nu_red[ii]+nx_red[ii]);
			qp_red->diag_H_flag[ii] = qp->diag_H_flag[ii];
			// TODO idxe !!!!!!!!!!!!!!!
			}
		else // copy everything
			{
			// copy vectors which are contiguous in the QP (e.g. to alias to res)
			if(ii<N)
				{
				VECCP(nx[ii+1], qp->b+ii, 0, qp_red->b+ii, 0);
				}
			VECCP(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0, qp_red->rqz+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0, qp_red->d+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d_mask+ii, 0, qp_red->d_mask+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->m+ii, 0, qp_red->m+ii, 0);
			qp_red->diag_H_flag[ii] = qp->diag_H_flag[ii];
			if(arg->alias_unchanged)
				{
				if(ii<N)
					{
					qp_red->BAbt[ii] = qp->BAbt[ii];
					}
				qp_red->RSQrq[ii] = qp->RSQrq[ii];
				qp_red->Z[ii] = qp->Z[ii];
				qp_red->idxb[ii] = qp->idxb[ii];
				qp_red->DCt[ii] = qp->DCt[ii];
				qp_red->idxs_rev[ii] = qp->idxs_rev[ii];
				qp_red->idxe[ii] = qp->idxe[ii];
				}
			else
				{
				if(ii<N)
					{
					GECP(nu[ii]+nx[ii]+1, nx[ii+1], qp->BAbt+ii, 0, 0, qp_red->BAbt+ii, 0, 0);
					}
				GECP(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0, qp_red->RSQrq+ii, 0, 0);
				VECCP(2*ns[ii], qp->Z+ii, 0, qp_red->Z+ii, 0);
				for(jj=0; jj<nb[ii]; jj++)
					qp_red->idxb[ii][jj] = qp->idxb[ii][jj];
				GECP(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0, qp_red->DCt+ii, 0, 0);
				for(jj=0; jj<nb[ii]+ng[ii]; jj++)
					qp_red->idxs_rev[ii][jj] = qp->idxs_rev[ii][jj];
				for(jj=0; jj<nbue[ii]+nbxe[ii]+nge[ii]; jj++)
					qp_red->idxe[ii][jj] = qp->idxe[ii][jj];
				}
			}
		}

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_LHS(struct OCP_QP *qp, struct OCP_QP *qp_red, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, struct OCP_QP_REDUCE_EQ_DOF_WS *work)
	{

	int ii, jj, kk, idx0, idx1;

	struct OCP_QP_DIM *dim = qp->dim;
	int N = dim->N;
	int *nx = dim->nx;
	int *nu = dim->nu;
	int *nb = dim->nb;
	int *nbx = dim->nbx;
	int *nbu = dim->nbu;
	int *ng = dim->ng;
	int *ns = dim->ns;
	int *nbue = dim->nbue;
	int *nbxe = dim->nbxe;
	int *nge = dim->nge;

	struct OCP_QP_DIM *dim_red = qp_red->dim;
	int *nx_red = dim_red->nx;
	int *nu_red = dim_red->nu;
	int *nb_red = dim_red->nb;
	int *ng_red = dim_red->ng;
	int *ns_red = dim_red->ns;

	// TODO handle case of softed equalities !!!!!!!!!!!!!!!!

	int ne_thr;

	for(ii=0; ii<=N; ii++)
		{
		if(ii==0)
			ne_thr = nbue[ii]+nbxe[ii];
		else
			ne_thr = nbue[ii];
		if(ne_thr>0) // reduce inputs and/or states
			{
			VECSE(nu[ii]+nx[ii], 0.0, work->tmp_nuxM+0, 0);
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				work->e_imask_ux[jj] = 0;
			for(jj=0; jj<nbu[ii]+nbx[ii]; jj++)
				work->e_imask_d[jj] = 0;
			for(jj=0; jj<ne_thr; jj++) // set 1s for both inputs and states
				{
				VECEL(work->tmp_nuxM+0, qp->idxb[ii][qp->idxe[ii][jj]]) = VECEL(qp->d+ii, qp->idxe[ii][jj]);
				work->e_imask_ux[qp->idxb[ii][qp->idxe[ii][jj]]] = 1;
				work->e_imask_d[qp->idxe[ii][jj]] = 1;
				}
			// TODO check first and last non-zero in e_mask and only multiply between them
			if(ii<N)
				{
				// BAt
				idx0 = 0;
				for(jj=0; jj<nu[ii]+nx[ii]; jj++)
					{
					if(work->e_imask_ux[jj]==0)
						{
						GECP(1, nx[ii+1], qp->BAbt+ii, jj, 0, qp_red->BAbt+ii, idx0, 0);
						idx0++;
						}
					}
				}
			// RSQ
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
//					idx1 = 0;
					idx1 = idx0;
//					for(kk=0; kk<nu[ii]+nx[ii]; kk++)
					for(kk=jj; kk<nu[ii]+nx[ii]; kk++)
						{
						if(work->e_imask_ux[kk]==0)
							{
							MATEL(qp_red->RSQrq+ii, idx1, idx0) = MATEL(qp->RSQrq+ii, kk, jj);
//#endif
							idx1++;
							}
						}
					idx0++;
					}
				}
			// idxb idxs_rev
			idx0 = 0;
			for(jj=0; jj<nb[ii]; jj++)
				{
				if(work->e_imask_d[jj]==0)
					{
					qp_red->idxb[ii][idx0] = qp->idxb[ii][jj];
					qp_red->idxs_rev[ii][idx0] = qp->idxs_rev[ii][jj]; // keep softed inequality constr with same slack
					idx0++;
					}
				}
			// DCt
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
					GECP(1, ng[ii], qp->DCt+ii, jj, 0, qp_red->DCt+ii, idx0, 0);
					idx0++;
					}
				}
			// soft constraints
			for(jj=0; jj<ng[ii]; jj++)
				qp_red->idxs_rev[ii][nb_red[ii]+jj] = qp->idxs_rev[ii][nb[ii]+jj]; // keep softed inequality constr with same slack
			VECCP(2*ns[ii], qp->Z+ii, 0, qp_red->Z+ii, 0);
			qp_red->diag_H_flag[ii] = qp->diag_H_flag[ii];
			// TODO idxe !!!!!!!!!!!!!!!
			}
		else // copy everything
			{
			qp_red->diag_H_flag[ii] = qp->diag_H_flag[ii];
			if(arg->alias_unchanged)
				{
				if(ii<N)
					{
					qp_red->BAbt[ii] = qp->BAbt[ii];
					}
				qp_red->RSQrq[ii] = qp->RSQrq[ii];
				qp_red->Z[ii] = qp->Z[ii];
				qp_red->idxb[ii] = qp->idxb[ii];
				qp_red->DCt[ii] = qp->DCt[ii];
				qp_red->idxs_rev[ii] = qp->idxs_rev[ii];
				qp_red->idxe[ii] = qp->idxe[ii];
				}
			else
				{
				if(ii<N)
					{
					GECP(nu[ii]+nx[ii]+1, nx[ii+1], qp->BAbt+ii, 0, 0, qp_red->BAbt+ii, 0, 0);
					}
				GECP(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0, qp_red->RSQrq+ii, 0, 0);
				VECCP(2*ns[ii], qp->Z+ii, 0, qp_red->Z+ii, 0);
				for(jj=0; jj<nb[ii]; jj++)
					qp_red->idxb[ii][jj] = qp->idxb[ii][jj];
				GECP(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0, qp_red->DCt+ii, 0, 0);
				for(jj=0; jj<nb[ii]+ng[ii]; jj++)
					qp_red->idxs_rev[ii][jj] = qp->idxs_rev[ii][jj];
				for(jj=0; jj<nbue[ii]+nbxe[ii]+nge[ii]; jj++)
					qp_red->idxe[ii][jj] = qp->idxe[ii][jj];
				}
			}
		}

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_RHS(struct OCP_QP *qp, struct OCP_QP *qp_red, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, struct OCP_QP_REDUCE_EQ_DOF_WS *work)
	{

	int ii, jj, kk, idx0, idx1;

	struct OCP_QP_DIM *dim = qp->dim;
	int N = dim->N;
	int *nx = dim->nx;
	int *nu = dim->nu;
	int *nb = dim->nb;
	int *nbx = dim->nbx;
	int *nbu = dim->nbu;
	int *ng = dim->ng;
	int *ns = dim->ns;
	int *nbue = dim->nbue;
	int *nbxe = dim->nbxe;
	int *nge = dim->nge;

	struct OCP_QP_DIM *dim_red = qp_red->dim;
	int *nx_red = dim_red->nx;
	int *nu_red = dim_red->nu;
	int *nb_red = dim_red->nb;
	int *ng_red = dim_red->ng;
	int *ns_red = dim_red->ns;

	// TODO handle case of softed equalities !!!!!!!!!!!!!!!!

	int ne_thr;

	for(ii=0; ii<=N; ii++)
		{
		if(ii==0)
			ne_thr = nbue[ii]+nbxe[ii];
		else
			ne_thr = nbue[ii];
		if(ne_thr>0) // reduce inputs and/or states
			{
			VECSE(nu[ii]+nx[ii], 0.0, work->tmp_nuxM+0, 0);
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				work->e_imask_ux[jj] = 0;
			for(jj=0; jj<nbu[ii]+nbx[ii]; jj++)
				work->e_imask_d[jj] = 0;
			for(jj=0; jj<ne_thr; jj++) // set 1s for both inputs and states
				{
				VECEL(work->tmp_nuxM+0, qp->idxb[ii][qp->idxe[ii][jj]]) = VECEL(qp->d+ii, qp->idxe[ii][jj]);
				work->e_imask_ux[qp->idxb[ii][qp->idxe[ii][jj]]] = 1;
				work->e_imask_d[qp->idxe[ii][jj]] = 1;
				}
			// TODO check first and last non-zero in e_mask and only multiply between them
			if(ii<N)
				{
				// b
				GEMV_T(nu[ii]+nx[ii], nx[ii+1], 1.0, qp->BAbt+ii, 0, 0, work->tmp_nuxM+0, 0, 1.0, qp->b+ii, 0, qp_red->b+ii, 0);
				}
			// rq
			SYMV_L(nu[ii]+nx[ii], 1.0, qp->RSQrq+ii, 0, 0, work->tmp_nuxM+0, 0, 1.0, qp->rqz+ii, 0, work->tmp_nuxM+1, 0);
			idx0 = 0;
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				{
				if(work->e_imask_ux[jj]==0)
					{
					VECEL(qp_red->rqz+ii, idx0) = VECEL(work->tmp_nuxM+1, jj);
					idx0++;
					}
				}
			// d d_mask m idxb idxs_rev
			idx0 = 0;
			for(jj=0; jj<nb[ii]; jj++)
				{
				if(work->e_imask_d[jj]==0)
					{
					VECEL(qp_red->d+ii, idx0) = VECEL(qp->d+ii, jj);
					VECEL(qp_red->d+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->d+ii, nb[ii]+ng[ii]+jj);
					VECEL(qp_red->d_mask+ii, idx0) = VECEL(qp->d_mask+ii, jj);
					VECEL(qp_red->d_mask+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->d_mask+ii, nb[ii]+ng[ii]+jj);
					VECEL(qp_red->m+ii, idx0) = VECEL(qp->m+ii, jj);
					VECEL(qp_red->m+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp->m+ii, nb[ii]+ng[ii]+jj);
					idx0++;
					}
				}
			VECCP(ng[ii], qp->d+ii, nb[ii], qp_red->d+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->d+ii, 2*nb[ii]+ng[ii], qp_red->d+ii, 2*nb_red[ii]+ng_red[ii]);
			VECCP(ng[ii], qp->d_mask+ii, nb[ii], qp_red->d_mask+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->d_mask+ii, 2*nb[ii]+ng[ii], qp_red->d_mask+ii, 2*nb_red[ii]+ng_red[ii]);
			VECCP(ng[ii], qp->m+ii, nb[ii], qp_red->m+ii, nb_red[ii]);
			VECCP(ng[ii]+2*ns[ii], qp->m+ii, 2*nb[ii]+ng[ii], qp_red->m+ii, 2*nb_red[ii]+ng_red[ii]);
			GEMV_T(nu[ii]+nx[ii], ng[ii], 1.0, qp->DCt+ii, 0, 0, work->tmp_nuxM+0, 0, 0.0, work->tmp_nbgM, 0, work->tmp_nbgM, 0);
			AXPY(ng[ii], -1.0, work->tmp_nbgM, 0, qp->d+ii, nb[ii], qp_red->d+ii, nb_red[ii]);
			AXPY(ng[ii], 1.0, work->tmp_nbgM, 0, qp->d+ii, 2*nb[ii]+ng[ii], qp_red->d+ii, 2*nb_red[ii]+ng_red[ii]);
			// soft constraints
			VECCP(2*ns[ii], qp->rqz+ii, nu[ii]+nx[ii], qp_red->rqz+ii, nu_red[ii]+nx_red[ii]);
			// TODO idxe !!!!!!!!!!!!!!!
			}
		else // copy everything
			{
			// copy vectors which are contiguous in the QP (e.g. to alias to res)
			if(ii<N)
				{
				VECCP(nx[ii+1], qp->b+ii, 0, qp_red->b+ii, 0);
				}
			VECCP(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0, qp_red->rqz+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0, qp_red->d+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d_mask+ii, 0, qp_red->d_mask+ii, 0);
			VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->m+ii, 0, qp_red->m+ii, 0);
			}
		}

	return;

	}



void OCP_QP_RESTORE_EQ_DOF(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol_red, struct OCP_QP_SOL *qp_sol, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, struct OCP_QP_REDUCE_EQ_DOF_WS *work)
	{

	int ii, jj, idx0;

	struct OCP_QP_DIM *dim = qp->dim;
	int N = dim->N;
	int *nx = dim->nx;
	int *nu = dim->nu;
	int *nb = dim->nb;
	int *nbx = dim->nbx;
	int *nbu = dim->nbu;
	int *ng = dim->ng;
	int *ns = dim->ns;
	int *nbue = dim->nbue;
	int *nbxe = dim->nbxe;
	int *nge = dim->nge;

	struct OCP_QP_DIM *dim_red = qp_sol_red->dim;
	int *nx_red = dim_red->nx;
	int *nu_red = dim_red->nu;
	int *nb_red = dim_red->nb;
	int *ng_red = dim_red->ng;

	int ne_thr;

	REAL tmp;

	for(ii=0; ii<=N; ii++)
		{
		if(ii==0)
			ne_thr = nbue[ii]+nbxe[ii];
		else
			ne_thr = nbue[ii];
		if(ne_thr>0) // restore inputs and/or states
			{
			VECSE(nu[ii]+nx[ii], 0.0, work->tmp_nuxM+0, 0);
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				work->e_imask_ux[jj] = 0;
			for(jj=0; jj<nb[ii]; jj++)
				work->e_imask_d[jj] = 0;
			for(jj=0; jj<ne_thr; jj++) // set 1s for both inputs and states
				{
				work->e_imask_ux[qp->idxb[ii][qp->idxe[ii][jj]]] = 1;
				work->e_imask_d[qp->idxe[ii][jj]] = 1;
				}
			// ux
			if(arg->comp_prim_sol)
				{
				idx0 = 0;
				for(jj=0; jj<nu[ii]+nx[ii]; jj++)
					{
					if(work->e_imask_ux[jj]==0)
						{
						VECEL(qp_sol->ux+ii, jj) = VECEL(qp_sol_red->ux+ii, idx0);
						idx0++;
						}
					}
				for(jj=0; jj<ne_thr; jj++)
					{
					VECEL(qp_sol->ux+ii, qp->idxb[ii][qp->idxe[ii][jj]]) = VECEL(qp->d+ii, qp->idxe[ii][jj]);
					}
				// TODO update based on choices on reduce !!!!!!!!!!!!!
				VECCP(2*ns[ii], qp_sol_red->ux+ii, nu_red[ii]+nx_red[ii], qp_sol->ux+ii, nu[ii]+nx[ii]);
				}
			if(arg->comp_dual_sol_eq)
				{
				// pi
				if(ii<N)
					VECCP(nx[ii+1], qp_sol_red->pi+ii, 0, qp_sol->pi+ii, 0);
				}
			if(arg->comp_dual_sol_ineq)
				{
				// lam t
				idx0 = 0;
				for(jj=0; jj<nb[ii]; jj++)
					{
					if(work->e_imask_d[jj]==0)
						{
						VECEL(qp_sol->lam+ii, jj) = VECEL(qp_sol_red->lam+ii, idx0);
						VECEL(qp_sol->lam+ii, nb[ii]+ng[ii]+jj) = VECEL(qp_sol_red->lam+ii, nb_red[ii]+ng_red[ii]+idx0);
						VECEL(qp_sol->t+ii, jj) = VECEL(qp_sol_red->t+ii, idx0);
						VECEL(qp_sol->t+ii, nb[ii]+ng[ii]+jj) = VECEL(qp_sol_red->t+ii, nb_red[ii]+ng_red[ii]+idx0);
						idx0++;
						}
					else
						{
						// lam
						// t
						VECEL(qp_sol->lam+ii, jj) = arg->lam_min;
						VECEL(qp_sol->lam+ii, nb[ii]+ng[ii]+jj) = arg->lam_min;
						VECEL(qp_sol->t+ii, jj) = arg->t_min;
						VECEL(qp_sol->t+ii, nb[ii]+ng[ii]+jj) = arg->t_min;
						}
					}
				// TODO update based on choices on reduce !!!!!!!!!!!!!
				VECCP(ng[ii], qp_sol_red->lam+ii, nb_red[ii], qp_sol->lam+ii, nb[ii]);
				VECCP(ng[ii]+2*ns[ii], qp_sol_red->lam+ii, 2*nb_red[ii]+ng_red[ii], qp_sol->lam+ii, 2*nb[ii]+ng[ii]);
				VECCP(ng[ii], qp_sol_red->t+ii, nb_red[ii], qp_sol->t+ii, nb[ii]);
				VECCP(ng[ii]+2*ns[ii], qp_sol_red->t+ii, 2*nb_red[ii]+ng_red[ii], qp_sol->t+ii, 2*nb[ii]+ng[ii]);
				// update lam_lb for removed eq if > 0, or lam_ub if < 0 //, keep lam_ub to zero
				VECCP(nu[ii]+nx[ii], qp->rqz+ii, 0, work->tmp_nuxM, 0);
				AXPY(nb[ii]+ng[ii], -1.0, qp_sol->lam+ii, 0, qp_sol->lam+ii, nb[ii]+ng[ii], work->tmp_nbgM, 0);
				VECAD_SP(nb[ii], 1.0, work->tmp_nbgM, 0, qp->idxb[ii], work->tmp_nuxM, 0);
				SYMV_L(nu[ii]+nx[ii], 1.0, qp->RSQrq+ii, 0, 0, qp_sol->ux+ii, 0, 1.0, work->tmp_nuxM, 0, work->tmp_nuxM, 0);
				if(ii<N)
					GEMV_N(nu[ii]+nx[ii], nx[ii+1], 1.0, qp->BAbt+ii, 0, 0, qp_sol_red->pi+ii, 0, 1.0, work->tmp_nuxM, 0, work->tmp_nuxM, 0);
				GEMV_N(nu[ii]+nx[ii], ng[ii], 1.0, qp->DCt+ii, 0, 0, work->tmp_nbgM, nb[ii], 1.0, work->tmp_nuxM, 0, work->tmp_nuxM, 0);
				for(jj=0; jj<nb[ii]; jj++)
					{
					if(work->e_imask_d[jj]!=0)
						{
						tmp = VECEL(work->tmp_nuxM, qp->idxb[ii][jj]);
						if(tmp>=0)
							VECEL(qp_sol->lam+ii, jj) = tmp;
						else
							VECEL(qp_sol->lam+ii, nb[ii]+ng[ii]+jj) = - tmp;
						}
					}
				}
			}
		else // copy
			{
			if(arg->comp_prim_sol)
				{
				// ux
				VECCP(nu[ii]+nx[ii]+2*ns[ii], qp_sol_red->ux+ii, 0, qp_sol->ux+ii, 0);
				}
			if(arg->comp_dual_sol_eq)
				{
				// pi
				if(ii<N)
					VECCP(nx[ii+1], qp_sol_red->pi+ii, 0, qp_sol->pi+ii, 0);
				}
			if(arg->comp_dual_sol_ineq)
				{
				// lam
				VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp_sol_red->lam+ii, 0, qp_sol->lam+ii, 0);
				// t
				VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp_sol_red->t+ii, 0, qp_sol->t+ii, 0);
				}
			}
		}

	return;

	}



void OCP_QP_REDUCE_EQ_DOF_SOL(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP_SOL *qp_sol_red, struct OCP_QP_REDUCE_EQ_DOF_ARG *arg, struct OCP_QP_REDUCE_EQ_DOF_WS *work)
	{

	int ii, jj, idx0;

	struct OCP_QP_DIM *dim = qp->dim;
	int N = dim->N;
	int *nx = dim->nx;
	int *nu = dim->nu;
	int *nb = dim->nb;
	int *nbx = dim->nbx;
	int *nbu = dim->nbu;
	int *ng = dim->ng;
	int *ns = dim->ns;
	int *nbue = dim->nbue;
	int *nbxe = dim->nbxe;
	int *nge = dim->nge;

	struct OCP_QP_DIM *dim_red = qp_sol_red->dim;
	int *nx_red = dim_red->nx;
	int *nu_red = dim_red->nu;
	int *nb_red = dim_red->nb;
	int *ng_red = dim_red->ng;

	int ne_thr;

	REAL tmp;

	for(ii=0; ii<=N; ii++)
		{
		if(ii==0)
			ne_thr = nbue[ii]+nbxe[ii];
		else
			ne_thr = nbue[ii];
		if(ne_thr>0) // restore inputs and/or states
			{
			VECSE(nu[ii]+nx[ii], 0.0, work->tmp_nuxM+0, 0);
			for(jj=0; jj<nu[ii]+nx[ii]; jj++)
				work->e_imask_ux[jj] = 0;
			for(jj=0; jj<nb[ii]; jj++)
				work->e_imask_d[jj] = 0;
			for(jj=0; jj<ne_thr; jj++) // set 1s for both inputs and states
				{
				work->e_imask_ux[qp->idxb[ii][qp->idxe[ii][jj]]] = 1;
				work->e_imask_d[qp->idxe[ii][jj]] = 1;
				}
			// ux
			if(arg->comp_prim_sol)
				{
				idx0 = 0;
				for(jj=0; jj<nu[ii]+nx[ii]; jj++)
					{
					if(work->e_imask_ux[jj]==0)
						{
						VECEL(qp_sol_red->ux+ii, idx0) = VECEL(qp_sol->ux+ii, jj);
						idx0++;
						}
					}
				// TODO update based on choices on reduce !!!!!!!!!!!!!
				VECCP(2*ns[ii], qp_sol->ux+ii, nu[ii]+nx[ii], qp_sol_red->ux+ii, nu_red[ii]+nx_red[ii]);
				}
			if(arg->comp_dual_sol_eq)
				{
				// pi
				if(ii<N)
					VECCP(nx[ii+1], qp_sol->pi+ii, 0, qp_sol_red->pi+ii, 0);
				}
			if(arg->comp_dual_sol_ineq)
				{
				// lam t
				idx0 = 0;
				for(jj=0; jj<nb[ii]; jj++)
					{
					if(work->e_imask_d[jj]==0)
						{
						VECEL(qp_sol_red->lam+ii, idx0) = VECEL(qp_sol->lam+ii, jj);
						VECEL(qp_sol_red->lam+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp_sol->lam+ii, nb[ii]+ng[ii]+jj);
						VECEL(qp_sol_red->t+ii, idx0) = VECEL(qp_sol->t+ii, jj);
						VECEL(qp_sol_red->t+ii, nb_red[ii]+ng_red[ii]+idx0) = VECEL(qp_sol->t+ii, nb[ii]+ng[ii]+jj);
						idx0++;
						}
					}
				// TODO update based on choices on reduce !!!!!!!!!!!!!
				VECCP(ng[ii], qp_sol->lam+ii, nb[ii], qp_sol_red->lam+ii, nb_red[ii]);
				VECCP(ng[ii]+2*ns[ii], qp_sol->lam+ii, 2*nb[ii]+ng[ii], qp_sol_red->lam+ii, 2*nb_red[ii]+ng_red[ii]);
				VECCP(ng[ii], qp_sol->t+ii, nb[ii], qp_sol_red->t+ii, nb_red[ii]);
				VECCP(ng[ii]+2*ns[ii], qp_sol->t+ii, 2*nb[ii]+ng[ii], qp_sol_red->t+ii, 2*nb_red[ii]+ng_red[ii]);
				}
			}
		else // copy
			{
			if(arg->comp_prim_sol)
				{
				// ux
				VECCP(nu[ii]+nx[ii]+2*ns[ii], qp_sol->ux+ii, 0, qp_sol_red->ux+ii, 0);
				}
			if(arg->comp_dual_sol_eq)
				{
				// pi
				if(ii<N)
					VECCP(nx[ii+1], qp_sol->pi+ii, 0, qp_sol_red->pi+ii, 0);
				}
			if(arg->comp_dual_sol_ineq)
				{
				// lam
				VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp_sol->lam+ii, 0, qp_sol_red->lam+ii, 0);
				// t
				VECCP(2*nb[ii]+2*ng[ii]+2*ns[ii], qp_sol->t+ii, 0, qp_sol_red->t+ii, 0);
				}
			}
		}

	return;

	}


